
namespace Kaleta.Xna.Collision
{
    // .NET Fx
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.Diagnostics;

    // XNA
    using Microsoft.Xna.Framework;

    public partial class Sap : ICollection<IBody>
    {
        #region Fields
        // Constants
        private const int DefaultCapacity = 500;
        private IBody[] bodies;
        private int count;
        private readonly IComparer<IBody> comparer;
        #endregion

        public CollisionBinding CollisionBinding { get; set; }

        public Sap()
        {
            this.count = 0;
            this.bodies = new IBody[Sap.DefaultCapacity];
            this.comparer = new GridSort();
            this.CollisionBinding = new CollisionBinding();
        }

        #region Methods
        /// <summary>Adds the elements of the specified collection to the broadphase</summary>
        public void AddRange(IEnumerable<IBody> collection)
        {
            #region Assertions
            Debug.Assert(collection != null);
            #endregion

            // Call our regular Add method
            foreach (var item in collection)
                this.Add(item);
        }

        /// <summary>A manual search that will work even when the bodies array is unsorted - use in place of BinarySearch as needed</summary>
        private int IndexOf(IBody item)
        {
            // Step through the entire bodies array looking for the index of this body
            if (this.count > 0)
                for (var index = 0; index < this.count; index++)
                {
                    if (object.ReferenceEquals(this.bodies[index], item))
                        return index;
                }

            // Not found
            return -1;
        }

        public int Index(IBody item)
        {
            return Array.BinarySearch(this.bodies, 0, this.count, item, this.comparer);
        }

        public void Update()
        {
            Sort();

            List<IBody> obstacles; 
            List<BodyShape> colliderShapeList; 
            List<BodyShape> obstacleShapeList;

            foreach (var collider in GetEnumerator())
            {
                obstacles = GetCollidingBodies(collider);

                foreach (var obstacle in obstacles)
                {
                    obstacle.Body.GetIntersectingShapes(collider.Body, out obstacleShapeList, out colliderShapeList);

                    obstacle.OnCollision(collider, obstacleShapeList, colliderShapeList);
                    collider.OnCollision(obstacle, colliderShapeList, obstacleShapeList);
                }
            }
        }

        public List<IBody> GetCollidingBodies(IBody collider)
        {
            #region Assertions
            Debug.Assert(collider != null);
            #endregion

            // Find the index for this body
            var index = Array.BinarySearch(this.bodies, 0, this.count, collider, this.comparer);

            // We should always get an index - Find out why we failed
            if (index < 0)
            {
                if (this.IndexOf(collider) < 0)
                    // This body just isn't here - I know, I know... But I looked through it manually!
                    throw new InvalidOperationException("The given body does not exist in this collection");
                else
                    // The collection has become unsorted due to either misuse of the API, or a body
                    // has exploded into an area of the number line that fails floating point comparison
                    throw new InvalidOperationException("This collection is not sorted");
            }

            List<IBody> obstacles = new List<IBody>();

            for (int n = index + 1; n < this.count; n++)
            {
                IBody obstacle = this.bodies[n];

                //make sure to handle colliding only for those objects that are supposed to collide (CollisionGroup):
                if (!this.CollisionBinding.Collides(collider.CollisionGroup, obstacle.CollisionGroup))
                    continue;

                if ((int)collider.Body.Position.Y / CELL_SIZE < (int)obstacle.Body.Position.Y / CELL_SIZE)
                    return obstacles;

                if (collider.Body.Position.X + collider.Body.BoundingBox.Max.X * collider.Body.Scale.X < obstacle.Body.Position.X - obstacle.Body.BoundingBox.Max.X * obstacle.Body.Scale.X)
                    return obstacles;


                if (collider.Body.Intersects(obstacle.Body))
                    obstacles.Add(obstacle);
            }

            return obstacles;
        }


        /// <summary>Finds all bodies contained by this collection</summary>
        public Enumerator GetEnumerator()
        {
            return new Enumerator(this);
        }


        /// <summary>Sorts the underlying lists that this collection relies upon</summary>
        /// <remarks>This method must be called before the FindInterference or Query methods are called</remarks>
        public void Sort()
        {
            // Filter: Skip this logic when we have no bodies
            if (this.count == 0)
                return;

            Array.Sort(this.bodies, 0, this.count, this.comparer);
        }
        #endregion

        #region ICollection<Body> Members
        /// <summary>Adds a body to the broadphase</summary>
        public void Add(IBody item)
        {
            #region Assertions
            Debug.Assert(item != null);
            Debug.Assert(this.IndexOf(item) < 0);
            #endregion

            var index = this.count;

            // Autogrow the arrays in large steps, only when needed
            this.count++;
            if (this.count > this.bodies.Length)
            {
                Array.Resize(ref this.bodies, this.bodies.Length * 2);
                //Array.Resize(ref this.markers, this.bodies.Length);
            }

            // Add this body to the array
            this.bodies[index] = item;
        }

        void ICollection<IBody>.Clear()
        {
            this.count = 0;
            this.bodies.Initialize();
        }

        /// <summary>Determines whether a body is in the broadphase</summary>
        bool ICollection<IBody>.Contains(IBody item)
        {
            #region Assertions
            Debug.Assert(item != null);
            #endregion

            // Try to find the index this body
            var index = Array.BinarySearch(this.bodies, 0, this.count, item, this.comparer);

            // If the array is not sorted; look harder...
            if (index < 0)
                index = this.IndexOf(item);

            // If BinarySearch/IndexOf returns -1 or less we didn't find it
            return index >= 0;
        }

        void ICollection<IBody>.CopyTo(IBody[] array, int arrayIndex)
        {
            Array.Copy(this.bodies, 0, array, arrayIndex, 0);
        }

        int ICollection<IBody>.Count
        {
            get { return this.count; }
        }

        bool ICollection<IBody>.IsReadOnly
        {
            get { return false; }
        }

        /// <summary>Removes a body from the broadphase</summary>
        public bool Remove(IBody item)
        {
            #region Assertions
            Debug.Assert(item != null);
            #endregion

            // Find the index of this body in our sorted array
            var index = Array.BinarySearch(this.bodies, 0, this.count, item, this.comparer);

            // If the array is not sorted; look harder...
            if (index < 0)
                index = this.IndexOf(item);

            // If we couldn't find this body just return false
            if (index < 0)
                return false;

            this.count--;

            // Shift all elements left after this item
            if (index < this.bodies.Length - 1)
            {
                Array.Copy(this.bodies, index + 1, this.bodies, index, this.count - index);
                //Array.Copy(this.markers, index + 1, this.markers, index, this.count - index);
            }

            // Reset the last elements (which is at this point a copy of the second to last element...)
            this.bodies[this.count] = null;

            return true;
        }
        #endregion

        #region IEnumerable<Body> Members
        IEnumerator<IBody> IEnumerable<IBody>.GetEnumerator()
        {
            return this.GetEnumerator();
        }

        IEnumerator IEnumerable.GetEnumerator()
        {
            return this.GetEnumerator();
        }
        #endregion
    }
}
